//------------------------------------------------------------
//  Filename: motor_driver.v
//   
//  Author  : wlduan@gmail.com
//  Revise  : 2016-09-27 18:26
//  Description: 
//   
//  Copyright (C) 2014, YRBD, Inc. 					      
//  All Rights Reserved.                                       
//-------------------------------------------------------------
//
`timescale 1ns/1ps
 
module motor_driver ( 
    input              clk,  
    input              rst,
    input  wire [15:0] speed_base,
    input  wire [31:0] motor_ctrl,
    input  wire [31:0] motor_move,

    output reg         force_pause,
    output reg  [3:0]  phase
);
//--------------------------------------------------------
reg [15:0] angle     ;
reg [5:0]  speed_used;
reg [5:0]  speed_want;
reg [7:0]  speed_sel ;
reg        cpu_ctrl  ;
reg        directoin ;
reg        pause     ;
reg        stop      ;
//--------------------------------------------------------
reg [31:0] speed_cntr;
//--------------------------------------------------------
reg[31:0] speed_move;
always @(posedge clk) cpu_ctrl   <= motor_ctrl[3];
always @(posedge clk) angle      <= (cpu_ctrl)? motor_ctrl[31:16]:motor_move[15:0];
always @(posedge clk) speed_sel  <= (cpu_ctrl)? motor_ctrl[15:8] :8'b1;
always @(posedge clk) speed_want <= (cpu_ctrl)? 6'b0             :motor_move[21:16];
always @(posedge clk) speed_used <= (cpu_ctrl)? 6'b0             :motor_move[27:22];
always @(posedge clk) stop       <= (cpu_ctrl)? motor_ctrl[0]    :motor_move[28];
always @(posedge clk) pause      <= (cpu_ctrl)? motor_ctrl[1]    :motor_move[29];
always @(posedge clk) directoin  <= (cpu_ctrl)? motor_ctrl[2]    :motor_move[30];
//--------------------------------------------------------
reg        pause_ff1 ;
reg        stop_ff1  ;
reg        start     ;
always @(posedge clk) pause_ff1 <= pause;
always @(posedge clk) stop_ff1  <= stop;
always @(posedge clk) start     <= (stop_ff1&(~stop))|(pause_ff1&(~pause));
wire[31:0] speed_need = speed_want*speed_base;  
wire       speed_get  = (speed_need[31:4] == speed_move[31:4])?1'b1:1'b0; // +/-16 difference
//--------------------------------------------------------
always @(posedge clk,posedge rst) begin
    if(rst)begin 
        speed_move <= 32'b0;    
    end 
    else if(cpu_ctrl)begin 
        speed_move <= speed_sel * speed_base;    
    end 
    else if(start||(speed_want == 0)) begin
        speed_move <= speed_used * speed_base;    
    end
    else if(speed_get) begin
        speed_move <= speed_move;    
    end
    else if(speed_cntr == 0) begin
        if(speed_want < speed_used) begin
            speed_move <= speed_move - 4; // max_step = 16
        end
        else begin
            speed_move <= speed_move + 4;
        end
    end
end 
//--------------------------------------------------------
always @(posedge clk,posedge rst) begin
    if(rst)begin 
        speed_cntr <= 32'b0;    
    end 
    else if(stop|pause|force_pause) begin 
        speed_cntr <= 32'b0;    
    end 
    else begin
        speed_cntr <= (speed_cntr < speed_move)?(speed_cntr + 1):32'b0;    
    end
end 
//--------------------------------------------------------
wire pwm_en = ((speed_cntr + 1) == speed_move)?1'b1:1'b0;
//--------------------------------------------------------
reg [15:0] angle_cntr;
//--------------------------------------------------------
always @(posedge clk,posedge rst) begin
    if(rst)begin 
        angle_cntr <= 0;        
    end 
    else if((stop)||(angle == 16'hffff)) begin
        angle_cntr <= 0;        
    end
    else if(pwm_en) begin 
        angle_cntr <= (angle_cntr < angle) ? (angle_cntr + 1):angle_cntr;    
    end 
end 
//--------------------------------------------------------
always @(posedge clk,posedge rst) begin
    if(rst)begin 
        force_pause <= 1'b0;    
    end 
    else if(angle_cntr == angle)begin 
        force_pause <= 1'b1;    
    end 
    else begin
        force_pause <= 1'b0;    
    end
end 
//--------------------------------------------------------
reg [2:0] step_cntr;
//--------------------------------------------------------
always @(posedge clk,posedge rst) begin
    if(rst)begin 
        step_cntr <= 0;
    end
    else if(pwm_en) begin
        if ( directoin ) begin
            step_cntr <= step_cntr - 1;
        end
        else begin
            step_cntr <= step_cntr + 1;
        end
    end
    else begin
        step_cntr <= step_cntr ;
    end
end
//--------------------------------------------------------
always @(posedge clk,posedge rst) begin
    if(rst)begin 
        phase <= 4'b1111;
    end
    else if(stop) begin
        phase <= 4'b1111;
    end
    else begin
        case(step_cntr)
            3'b000: phase <=4'b1110;
		    3'b001: phase <=4'b1100;
		    3'b010: phase <=4'b1101;
		    3'b011: phase <=4'b1001;
		    3'b100: phase <=4'b1011;
		    3'b101: phase <=4'b0011;
		    3'b110: phase <=4'b0111;
		    3'b111: phase <=4'b0110;
	    endcase
    end
end
endmodule
